#pragma once
#include "SLAM/common.hpp"

namespace SLAM {

// 用 2 对点快速解 SE2（平面刚体变换）
// 输入：obs_base 中的观测（机器人系），lms_map 中的地标（地图系），通过匹配索引对应
// 输出：平移(x,y)+yaw 与 RMSE 和内点集合
struct SE2Solution {
  double x{0}, y{0}, yaw{0};
  double rmse{1e9};
  std::vector<int> inliers;
};
// 输入：a:机器人系下向量坐标；b：地图系下向量坐标；
// 输出：机器人在地图系下的位姿（x,y,yaw）
inline bool solveSE2_min2(const std::vector<Eigen::Vector2d>& a,
                          const std::vector<Eigen::Vector2d>& b,
                          double& x, double& y, double& yaw)
{
  if (a.size()!=2 || b.size()!=2) return false;
  Eigen::Vector2d a0=a[0], a1=a[1], b0=b[0], b1=b[1];
  Eigen::Vector2d da=a1-a0, db=b1-b0;
  const double n_a = da.norm(), n_b = db.norm();
  if (n_a<1e-6 || n_b<1e-6) return false;
  const double ca = std::atan2(da.y(), da.x());
  const double cb = std::atan2(db.y(), db.x());
  yaw = cb - ca;
  const double c = std::cos(yaw), s = std::sin(yaw);
  // 机器人系坐标 转换为 地图系坐标
  Eigen::Vector2d Ra0(c*a0.x()-s*a0.y(), s*a0.x()+c*a0.y());
  Eigen::Vector2d t = b0 - Ra0;
  x = t.x(); y = t.y();
  return true;
}

inline SE2Solution ransacSE2(const std::vector<Reflector>& obs_base,
                             const std::vector<Landmark>&  lms_map,
                             const std::vector<MatchPair>& matches,
                             double inlier_thr=0.20,   // m
                             int    iters=100)
{
  SE2Solution best;

  // 准备对应点
  std::vector<Eigen::Vector2d> A, B;
  std::vector<int> idx_obs, idx_lm;
  for (auto &mp : matches) {
    if (mp.obs_index<0) continue;
    // base -> map 对应关系： apply T(a)=b
    A.emplace_back(obs_base[mp.obs_index].x, obs_base[mp.obs_index].y);
    // 找到对应 lm
    for (auto &L : lms_map) if (L.id==mp.lm_index) { B.emplace_back(L.x, L.y); break; }
  }
  if (A.size() < 3) return best;

  std::mt19937 rng(42);
  std::uniform_int_distribution<int> uni(0, (int)A.size()-1);

  for (int it=0; it<iters; ++it){
    // 采样 2 对
    int i0=uni(rng), i1=uni(rng);
    if (i0==i1) { --it; continue; }

    double x=0,y=0,yaw=0;
    if (!solveSE2_min2({A[i0],A[i1]}, {B[i0],B[i1]}, x,y,yaw)) continue;

    // 统计内点
    const double c=std::cos(yaw), s=std::sin(yaw);
    std::vector<int> inl;
    double se=0.0;
    for (size_t k=0;k<A.size();++k){
      Eigen::Vector2d Ra(c*A[k].x()-s*A[k].y()+x, s*A[k].x()+c*A[k].y()+y);
      const double e = (Ra - B[k]).norm();
      if (e <= inlier_thr){ inl.push_back((int)k); se += e*e; }
    }
    if (inl.size() < 3) continue;
    const double rmse = std::sqrt(se / std::max<size_t>(1,inl.size()));
    if (rmse < best.rmse){
      best.x=x; best.y=y; best.yaw=yaw; best.rmse=rmse; best.inliers=inl;
    }
  }
  return best;
}

} // namespace SLAM
